#include "odom_node.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"

#include "std_msgs/msg/int32_multi_array.hpp"
#include <deque>

class odom_node : public rclcpp::Node
{
  private:
	std::string _frame_id = "odom";
	std::string _child_frame_id = "base_footprint";
	double _wheel_base = 0.525, _track_width = 0.4, _wheel_diameter = 0.28;
	double _scale_pos = 163800, _scale_vel = 27.44;
	int sock;
	int _driver_model = 0;

	/**
	 * @brief convert motor raw binary feedback data to wheel centre point degree and velocity
	 *
	 * @param fb feedback data
	 * @param real wheel centre point attributes
	 * @param dt sample period
	 * @return int
	 */
	int bin_to_real(motor_feedback_t fb, point_real_t &real)
	{
		// to degree
		real.pos[0] = double(fb.fr_pos) / this->_scale_pos + 90;
		real.pos[1] = double(fb.br_pos) / this->_scale_pos + 90;
		real.pos[2] = double(fb.bl_pos) / this->_scale_pos + 90;
		real.pos[3] = double(fb.fl_pos) / this->_scale_pos + 90;
		// to wheel centre point velocity
		real.vel[0] = fb.fr_vel * this->_wheel_diameter * 3.14159 / this->_scale_vel;
		real.vel[1] = fb.br_vel * this->_wheel_diameter * 3.14159 / this->_scale_vel;
		real.vel[2] = fb.bl_vel * this->_wheel_diameter * 3.14159 / this->_scale_vel;
		real.vel[3] = fb.fl_vel * this->_wheel_diameter * 3.14159 / this->_scale_vel;
		return 0;
	}

	int driver_feedback(motor_feedback_t &fb)
	{
		char buf[1024];
		int len;
		if (this->sock < 0)
			return 0;
		struct sockaddr_in src_addr;
		socklen_t addr_len = sizeof(src_addr);
		len = recvfrom(this->sock, buf, 1024, 0, (struct sockaddr*)&src_addr, &addr_len);
		if (len < 0 || len > 1024)
		{
			return errno == EAGAIN ? 0 : -errno;
		}
		else
		{
			if (src_addr.sin_addr.s_addr != remote_addr.sin_addr.s_addr)
			{
				return 0;
			}

			try
			{
				// add a terminating '\0', or parse may fail
				buf[len] = '\0';
				// printf("recv %d: %s\n", len, buf);
				nlohmann::json input = nlohmann::json::parse(buf);

				fb.fr_pos = input["fr_pos"];
				fb.fr_vel = input["fr_vel"];

				fb.br_pos = input["br_pos"];
				fb.br_vel = input["br_vel"];

				fb.bl_pos = input["bl_pos"];
				fb.bl_vel = input["bl_vel"];

				fb.fl_pos = input["fl_pos"];
				fb.fl_vel = input["fl_vel"];

				return 1;
			}
			catch (const nlohmann::detail::parse_error &e)
			{
				// parse error, ignore
				printf("caught parse error\n");
				return 0;
			}
		}
	}

	/**
	 * @brief calculate robot movement with motor feedback, suppose that robot movement was
	 * mixed with px, py, and self rotation theta
	 *
	 * @param fb motor feedback data
	 * @param pose_y body centre shift in y axis (is positive if moving forward), m
	 * @param pose_x body centre shift in x axis (is positive if moving to right side), m
	 * @param yaw self rotation, is positive if anticlockwise (top view), rad
	 * @param vel_y body centre velocity in y axis
	 * @param vel_x body centre velocity in x axis
	 * @param yawrate rotation speed
	 * @param dt sample time period, s
	 * @return int
	 */
	int odom_update_omni(motor_feedback_t &fb, double dt, double &pose_y, double &pose_x,
						 double &yaw, double &vel_y, double &vel_x, double &yawrate)
	{
		// body translation component x and y, and rotation speed
		double py[2], px[2], theta[4];
		// four wheel velocity components
		double vy[4], vx[4];
		int i;
		point_real_t real;
		// convert to proper unit
		this->bin_to_real(fb, real);
		// calculate axis component
		for (i = 0; i < 4; i++)
		{
			vy[i] = sin(real.pos[i] * 3.14159 / 180) * real.vel[i];
			vx[i] = cos(real.pos[i] * 3.14159 / 180) * real.vel[i];
		}
		// printf("realpos,realvel  %lf,%lf  %lf,%lf  %lf,%lf  %lf,%lf\n", real.pos[0],
		// real.vel[0], 	   real.pos[1], real.vel[1], real.pos[2], real.vel[2], real.pos[3],
		// real.vel[3]); printf("vx,vy  %lf,%lf  %lf,%lf  %lf,%lf  %lf,%lf\n", vx[0], vy[0],
		// vx[1], vy[1], vx[2], 	   vy[2], vx[3], vy[3]); add diagonal px and py to eliminate
		// rotation component
		py[0] = (vy[0] + vy[2]) / 2;
		py[1] = (vy[1] + vy[3]) / 2;
		px[0] = (vx[0] + vx[1]) / 2;
		px[1] = (vx[1] + vx[3]) / 2;
		// substract to eliminate px and py
		theta[0] = (vx[2] - vx[0]) / this->_wheel_base;
		theta[1] = (vx[1] - vx[3]) / this->_wheel_base;
		theta[2] = (vy[0] - vy[2]) / this->_track_width;
		theta[3] = (vy[1] - vy[3]) / this->_track_width;

		// average
		vel_y = (py[0] + py[1]) / 2;
		vel_x = (px[0] + px[1]) / 2;
		yawrate = (theta[0] + theta[1] + theta[2] + theta[3]) / 4;
		//
		pose_y = vel_y * dt;
		pose_x = vel_x * dt;
		yaw = yawrate * dt;

		return 0;
	}

	int odom_update_diff(motor_feedback_t &fb, double dt, double &pose_y, double &pose_x,
						 double &yaw, double &vel_y, double &vel_x, double &yawrate)
	{
		double left, right;
		return odom_update_diff(fb, dt, pose_y, pose_x, yaw, vel_y, vel_x, yawrate, left,
								right);
	}

	int odom_update_diff(motor_feedback_t &fb, double dt, double &pose_y, double &pose_x,
						 double &yaw, double &vel_y, double &vel_x, double &yawrate,
						 double &left_wheel_vel, double &right_wheel_vel)
	{
		point_real_t real;
		this->bin_to_real(fb, real);
		double vy[4];
		// double r = 0;
		for (int i = 0; i < 4; i++)
		{
			vy[i] = sin(real.pos[i] * 3.14159 / 180) * real.vel[i];
		}
		left_wheel_vel = (vy[2] + vy[3]) / 2;
		right_wheel_vel = (vy[0] + vy[1]) / 2;
		yawrate = (right_wheel_vel - left_wheel_vel) / this->_track_width;
		if (abs(yawrate) < 0.01)
		{
			vel_y = (right_wheel_vel + left_wheel_vel) / 2;
			vel_x = 0;
			yaw = yawrate * dt;
			pose_y = vel_y * dt;
			pose_x = 0;
		}
		else
		{
			vel_x = 0;
			vel_y = (right_wheel_vel + left_wheel_vel) / 2;

			// r = vel_y / yawrate;
			yaw = yawrate * dt;
			// pose_y = sin(yaw) * r;
			// pose_x = (cos(yaw) - 1) * r;  // no significant difference
			pose_y = vel_y * dt;
			pose_x = 0;
		}
		return 0;
	}

	int odom_update_ackerman(motor_feedback_t &fb, double dt, double &pose_y, double &pose_x,
							 double &yaw, double &vel_y, double &vel_x, double &yawrate)
	{
		point_real_t real;
		this->bin_to_real(fb, real);
		double vy[4], vx[4];
		for (int i = 0; i < 4; i++)
		{
			vy[i] = sin(real.pos[i] * 3.14159 / 180) * real.vel[i];
			vx[i] = cos(real.pos[i] * 3.14159 / 180) * real.vel[i];
		}
		double w = this->_track_width / 2;
		double l = this->_wheel_base / 2;
		double vy_l = (vy[3] + vy[2]) / 2;
		double vx_l = (vx[3] - vx[2]) / 2;
		double vy_r = (vy[0] + vy[1]) / 2;
		double vx_r = (vx[0] - vx[1]) / 2;
		double r, omega;
		printf("ackerman vy_l %lf, vx_l %lf, vy_r %lf, vx_r %lf", vy_l, vx_l, vy_r, vx_r);
		std::cout << std::endl;
		if (abs(vx_l) < 0.0001 && abs(vx_r) < 0.0001)
		{
			// straight forward or reverse, r -> inf
			vel_y = (vy_l + vy_r) / 2;
			vel_x = 0;
			yaw = 0;
			yawrate = 0;
			pose_y = vel_y * dt;
			pose_x = vel_x * dt;
			printf("ackerman r inf, omega 0, vel_y %lf, dt %lf \n", vel_y, dt);
			std::cout << std::endl;
			return 0;
		}
		if (abs(vy_l) < 0.001 && abs(vy_r) < 0.001)
		{
			vel_y = 0;
			vel_x = 0;
			yaw = 0;
			yawrate = 0;
			pose_y = 0;
			pose_x = 0;
			printf("ackerman r inf, omega 0, vel_y 0, dt %lf \n", dt);
			std::cout << std::endl;
			return 0;
		}

		r = ((l * abs(vy_l) / abs(vx_l) + w) + (l * abs(vy_r) / abs(vx_r) - w)) / 2;
		omega = ((abs(vx_l) / l) + (abs(vx_r) / l)) / 2;
		// vel_y = r * omega;
		vel_y = ((abs(vy_l) + w * abs(vx_l) / l) + (abs(vy_r) - w * abs(vx_r) / l)) / 2;
		if (vy_l > 0)
		{
			if (vx_l > 0) omega *= -1;
		}
		else
		{
			vel_y *= -1;
			if (vx_l > 0) omega *= -1;
		}
		vel_x = 0;
		pose_x = 0;
		pose_y = vel_y * dt;
		omega = omega * 0.75 + 0.03;
		yaw = omega * dt;
		yawrate = omega;
		printf("ackerman r %lf, omega %lf, vel_y %lf, dt %lf \n", r, omega, vel_y, dt);
		std::cout << std::endl;
		return 0;
	}

	void odom_thread_func()
	{
		nav_msgs::msg::Odometry odom;
		motor_feedback_t fb;
		int err;
		double dx, dy, dyaw, vx, vy, yawrate;
		tf2::Quaternion quad;
		auto lastsample = this->get_clock()->now();
		odom.pose.pose.position.x = 0;
		odom.pose.pose.position.y = 0;
		odom.pose.pose.position.z = 0;
		quad.setRPY(0, 0, 0);
		odom.pose.pose.orientation.x = quad.getX();
		odom.pose.pose.orientation.y = quad.getY();
		odom.pose.pose.orientation.z = quad.getZ();
		odom.pose.pose.orientation.w = quad.getW();
		// set covariance
		// see
		// https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py
		// ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
		//                         0, 1e-3, 0, 0, 0, 0,
		//                         0, 0, 1e6, 0, 0, 0,
		//                         0, 0, 0, 1e6, 0, 0,
		//                         0, 0, 0, 0, 1e6, 0,
		//                         0, 0, 0, 0, 0, 1e3]
		// ODOM_POSE_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0,
		//                          0, 1e-3, 1e-9, 0, 0, 0,
		//                          0, 0, 1e6, 0, 0, 0,
		//                          0, 0, 0, 1e6, 0, 0,
		//                          0, 0, 0, 0, 1e6, 0,
		//                          0, 0, 0, 0, 0, 1e-9]

		// ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
		//                          0, 1e-3, 0, 0, 0, 0,
		//                          0, 0, 1e6, 0, 0, 0,
		//                          0, 0, 0, 1e6, 0, 0,
		//                          0, 0, 0, 0, 1e6, 0,
		//                          0, 0, 0, 0, 0, 1e3]
		// ODOM_TWIST_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0,
		//                           0, 1e-3, 1e-9, 0, 0, 0,
		//                           0, 0, 1e6, 0, 0, 0,
		//                           0, 0, 0, 1e6, 0, 0,
		//                           0, 0, 0, 0, 1e6, 0,
		//                           0, 0, 0, 0, 0, 1e-9]
		tf2::Transform tf_his, tf_cur;
		tf_his.setOrigin(tf2::Vector3(0, 0, 0));
		tf_his.setRotation(tf2::Quaternion(0, 0, 0, 1));
		nav_msgs::msg::Path path;
		path.header.frame_id = this->_frame_id;
		int timeout_cnt = 0;
		while (rclcpp::ok())
		{
			if (this->from_ros)
			{
				if (!fblock.try_lock_for(std::chrono::milliseconds(10)))
				{
					RCLCPP_WARN(this->get_logger(), "timeout for odom thread when try lock");
					continue;
				}
				if (feedbacks.empty())
				{
					fblock.unlock();
					std::this_thread::sleep_for(std::chrono::milliseconds(10));
					continue;
				}
				fb = feedbacks.front();
				feedbacks.pop_front();
				fblock.unlock();
				err = 1;
			}
			else
				err = this->driver_feedback(fb);
			

			if (err > 0)
			{
				// got feedback
				auto thissample = this->get_clock()->now();
				double dt = (thissample - lastsample).seconds();
				odom.header.stamp = thissample;
				odom.header.frame_id = this->_frame_id;
				odom.child_frame_id = this->_child_frame_id;

				if (this->to_ros)
				{
					std_msgs::msg::Int32MultiArray raw;
					raw.data.resize(8);
					raw.data[0] = fb.fr_pos;
					raw.data[1] = fb.fr_vel;
					raw.data[2] = fb.br_pos;
					raw.data[3] = fb.br_vel;
					raw.data[4] = fb.bl_pos;
					raw.data[5] = fb.bl_vel;
					raw.data[6] = fb.fl_pos;
					raw.data[7] = fb.fl_vel;
					this->raw_pub->publish(raw);
				}

				// update odom
				if (this->_driver_model == 0)
				{
					this->odom_update_omni(fb, dt, dy, dx, dyaw, vy, vx, yawrate);
				}
				else if (this->_driver_model == 1)
				{
					geometry_msgs::msg::TwistStamped twist;
					this->odom_update_diff(fb, dt, dy, dx, dyaw, vy, vx, yawrate,
										   twist.twist.linear.x, twist.twist.linear.y);
					RCLCPP_DEBUG(this->get_logger(), "twist left %lf, right %lf",
								 twist.twist.linear.x, twist.twist.linear.y);
					twist.header.stamp = rclcpp::Clock().now();
					twist.header.frame_id = this->_child_frame_id;
					this->twist_pub->publish(twist);
				}
				else if (this->_driver_model == 2)
				{
					this->odom_update_ackerman(fb, dt, dy, dx, dyaw, vy, vx, yawrate);
				}
				// RCLCPP_DEBUG(this->get_logger(), "odom dy %lf, dx %lf, dyaw %lf", dy, dx,
				// dyaw);
				printf("odom dy %lf, dx %lf, dyaw %lf, dt %lf \n", dy, dx, dyaw, dt);
				quad.setRPY(0, 0, dyaw);
				lastsample = thissample;

				// in this period, transform was based on footprint, need convert to map
				tf_cur = tf2::Transform(quad, tf2::Vector3(dy, -dx, 0));
				tf_his *= tf_cur;

				// set pose
				odom.pose.pose.position.x = tf_his.getOrigin().getX();
				odom.pose.pose.position.y = tf_his.getOrigin().getY();
				odom.pose.pose.position.z = tf_his.getOrigin().getZ();
				odom.pose.pose.orientation.x = tf_his.getRotation().getX();
				odom.pose.pose.orientation.y = tf_his.getRotation().getY();
				odom.pose.pose.orientation.z = tf_his.getRotation().getZ();
				odom.pose.pose.orientation.w = tf_his.getRotation().getW();

				// set velocity
				odom.twist.twist.linear.x = vy;
				odom.twist.twist.linear.y = -vx;
				odom.twist.twist.linear.z = 0;
				odom.twist.twist.angular.x = 0;
				odom.twist.twist.angular.y = 0;
				odom.twist.twist.angular.z = yawrate;
				odom.pose.covariance = { 0.1, 0, 0, 0, 0, 0,
										  0, 0.1, 0, 0, 0, 0,
										  0, 0, 10, 0, 0, 0,
										  0, 0, 0, 10, 0, 0,
										  0, 0, 0, 0, 10, 0,
										  0, 0, 0, 0, 0, 0.1};
				odom.twist.covariance = {0.1, 0, 0, 0, 0, 0,
				                          0, 0.1, 0.1, 0, 0, 0,
				                          0, 0, 10, 0, 0, 0,
				                          0, 0, 0, 10, 0, 0,
				                          0, 0, 0, 0, 10, 0,
				                          0, 0, 0, 0, 0, 0.1};
				// publish
				this->odom_pub->publish(odom);
				// RCLCPP_INFO(this->get_logger(), "published odom message");
				// update transform between odom and footprint
				if (publish_tf)
				{
					geometry_msgs::msg::TransformStamped tf;
					tf.header.frame_id = this->_frame_id;
					tf.child_frame_id = this->_child_frame_id;
					tf.header.stamp = this->get_clock()->now();
					tf.transform.translation.x = odom.pose.pose.position.x;
					tf.transform.translation.y = odom.pose.pose.position.y;
					tf.transform.translation.z = odom.pose.pose.position.z;
					tf.transform.rotation = odom.pose.pose.orientation;
					this->tf_broadcaster->sendTransform(tf);
				}
				// publish path, this consumes about 4.8kB memory per second
				path.header.stamp = this->get_clock()->now();
				geometry_msgs::msg::PoseStamped pathnode;
				pathnode.header.frame_id = path.header.frame_id;
				pathnode.header.stamp = path.header.stamp;
				pathnode.pose = odom.pose.pose;
				path.poses.push_back(pathnode);
				this->path_pub->publish(path);
				timeout_cnt = 0;
			}
			else if (err == 0)
			{
				// socket return -1, EAGAIN, normal timeout
				timeout_cnt += 1;
				if (timeout_cnt > 100)
				{
					RCLCPP_WARN(this->get_logger(), "timeout, try reconnect to %s", remote_ip.c_str());
					socket_connect();
					timeout_cnt = 0;
				}
			}
			else
			{
				// socket return -1, errno != EAGAIN
				RCLCPP_WARN(this->get_logger(), "read feedback failed, return -1");
			}
		}
	}

	// topic publish
	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
	rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub;
	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;

	rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr raw_pub;
	rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr raw_sub;
	// thread to integrate odometry
	std::shared_ptr<std::thread> odom_thread;

	std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;

	std::string remote_ip = "127.0.0.1";
	std::string local_ip = "127.0.0.1";
	std::string model = "Omnidirectional";
	std::string odom_topic = "wheel_odom";
	std::string twist_topic = "wheel_twist";
	std::string path_topic = "wheel_path";
	struct sockaddr_in remote_addr, local_addr;
	bool from_ros = false;  // motor feedback data from ros message
	bool to_ros = false;  // publish feedback data to ros message
	bool publish_tf = true;

	std::timed_mutex fblock;
	std::deque<motor_feedback_t> feedbacks;

	int socket_connect()
	{
		this->sock = socket(AF_INET, SOCK_DGRAM, 0);
		if (this->sock < 0)
		{
			RCLCPP_INFO(this->get_logger(), "faild to init socket");
			return -1;
		}
		int err;
		err = 1;
		setsockopt(this->sock, SOL_SOCKET, SO_REUSEADDR, &err, sizeof(err));
		memset(&local_addr, 0x00, sizeof(local_addr));
		local_addr.sin_family = AF_INET;
		local_addr.sin_addr.s_addr = inet_addr(local_ip.c_str());
		local_addr.sin_port = htons(8001);
		err = bind(this->sock, (struct sockaddr *)&local_addr, sizeof(local_addr));
		if (err < 0)
		{
			RCLCPP_INFO(this->get_logger(), "failed to bind socket to port 8001");
			close(this->sock);
			this->sock = -1;
			return -1;
		}
		this->remote_addr.sin_family = AF_INET;
		this->remote_addr.sin_addr.s_addr = inet_addr(remote_ip.c_str());
		this->remote_addr.sin_port = htons(8005);

		struct timeval timeout = {0, 20000};
		setsockopt(this->sock, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
		RCLCPP_INFO(this->get_logger(), "socket connected");
		return 0;
	}

	void motor_feedback_callback(std_msgs::msg::Int32MultiArray msg)
	{
		motor_feedback_t fb;
		if (msg.data.size() < 8)
		{
			RCLCPP_WARN(this->get_logger(), "motor feedback message length not correct: %ld, expect 8", msg.data.size());
			return;
		}
		fb.fr_pos = msg.data[0];
		fb.fr_vel = msg.data[1];
		fb.br_pos = msg.data[2];
		fb.br_vel = msg.data[3];
		fb.bl_pos = msg.data[4];
		fb.bl_vel = msg.data[5];
		fb.fl_pos = msg.data[6];
		fb.fl_vel = msg.data[7];

		if (!fblock.try_lock_for(std::chrono::milliseconds(10)))
		{
			RCLCPP_WARN(this->get_logger(), "timeout for callback function when try lock");
			return;
		}

		feedbacks.push_back(fb);
		fblock.unlock();
	}

  public:
	odom_node(std::string name) : Node(name)
	{
		// declare parameters
		this->declare_parameter("wheel_base", _wheel_base);
		this->declare_parameter("track_width", _track_width);
		this->declare_parameter("wheel_diameter", _wheel_diameter);
		this->declare_parameter("scale_pos", _scale_pos);
		this->declare_parameter("scale_vel", _scale_vel);
		this->declare_parameter("remote_ip", remote_ip);
		this->declare_parameter("local_ip", local_ip);
		this->declare_parameter("frame_id", _frame_id);
		this->declare_parameter("child_frame_id", _child_frame_id);
		this->declare_parameter("odom_topic", odom_topic);
		this->declare_parameter("driver_model", model);
		this->declare_parameter("twist_topic", twist_topic);
		this->declare_parameter("path_topic", path_topic);
		this->declare_parameter("from_ros", from_ros);
		this->declare_parameter("to_ros", to_ros);
		this->declare_parameter("publish_tf", publish_tf);

		this->get_parameter("wheel_base", _wheel_base);
		this->get_parameter("track_width", _track_width);
		this->get_parameter("wheel_diameter", _wheel_diameter);
		this->get_parameter("scale_pos", _scale_pos);
		this->get_parameter("scale_vel", _scale_vel);
		this->get_parameter("remote_ip", remote_ip);
		this->get_parameter("local_ip", local_ip);
		this->get_parameter("frame_id", _frame_id);
		this->get_parameter("child_frame_id", _child_frame_id);
		this->get_parameter("odom_topic", odom_topic);
		this->get_parameter("driver_model", model);
		this->get_parameter("twist_topic", twist_topic);
		this->get_parameter("path_topic", path_topic);
		this->get_parameter("from_ros", from_ros);
		this->get_parameter("to_ros", to_ros);
		this->get_parameter("publish_tf", publish_tf);
		if (model == "Omnidirectional")
		{
			this->_driver_model = 0;
			RCLCPP_INFO(this->get_logger(), "driver model: Omnidirectional");
		}
		else if (model == "Differential")
		{
			this->_driver_model = 1;
			RCLCPP_INFO(this->get_logger(), "driver model: Differential");
		}
		else if (model == "Ackerman")
		{
			this->_driver_model = 2;
			RCLCPP_INFO(this->get_logger(), "driver model: Ackerman");
		}
		else
		{
			this->_driver_model = 0;
			RCLCPP_INFO(this->get_logger(),
						"unexpected driver model string, select Omnidirectional by default");
		}

		RCLCPP_INFO(this->get_logger(),
					"wheel_base: %lf, track_width: %lf, wheel_diameter: %lf, scale_pos: %lf, "
					"scale_vel: %lf, local_ip: %s",
					_wheel_base, _track_width, _wheel_diameter, _scale_pos, _scale_vel,
					local_ip.c_str());

		

		// odometry publisher
		this->odom_pub = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic, 100);
		RCLCPP_INFO(this->get_logger(), "publish odometry to topic: %s", odom_topic.c_str());
		if (this->_driver_model == 1)
		{
			this->twist_pub =
				this->create_publisher<geometry_msgs::msg::TwistStamped>(twist_topic, 100);
			RCLCPP_INFO(this->get_logger(),
						"publish differential wheel twist(left wheel in .linear.x, right wheel "
						"in .linear.y) to topic: %s",
						twist_topic.c_str());
		}

		if (this->from_ros)
		{
			RCLCPP_INFO(this->get_logger(), "from_ros enabled, will receive motor feedback data ONLY from ros");
			this->raw_sub = this->create_subscription<std_msgs::msg::Int32MultiArray>("wheel_raw", 100, std::bind(&odom_node::motor_feedback_callback, this, std::placeholders::_1));
		}
		else
		{
			socket_connect();
			RCLCPP_INFO(this->get_logger(), "socket prepared");
		}
		if (this->to_ros)
		{
			if (this->from_ros)
			{
				RCLCPP_WARN(this->get_logger(), "cannot receive and record ros feedback data at the same time, to_ros will be disabled");
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "to_ros enabled, will publish motor feedback data as ros message");
				this->raw_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("wheel_raw", 100);
			}
		}


		this->path_pub = this->create_publisher<nav_msgs::msg::Path>(path_topic, 100);
		if (publish_tf)
			// tf listener
			this->tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

		this->odom_thread =
			std::make_shared<std::thread>(std::bind(&odom_node::odom_thread_func, this));
		this->odom_thread->detach();
	}

	~odom_node()
	{
		close(this->sock);
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<odom_node>("odom_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
